﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Kinect;
using System.Windows;
using System.Windows.Shapes;
using System.Windows.Controls;
using System.Diagnostics;
using System.Windows.Markup;
using System.IO;
using System.Xml;
using KinectGestureRecognition.Configuration;
//using Fizbin.Kinect.Gestures;

namespace KinectGestureRecognition
{
    public class KinectSensorModul
    {

        #region members
        // Singelton variable
        private static KinectSensorModul instance;
        private WriteableBitmap _ColorImageBitmap;
        private Int32Rect _ColorImageBitmapRect;
        private int _ColorImageStride;
        private short[] _LastDepthPixelData;
        private SkeletonStreamConfiguration ssc;
        /// <summary>
        /// Drawing group for skeleton rendering output
        /// </summary>
        private DrawingGroup _drawingGroup;
        private ImageSource _depthImageSource;
        //private GestureController _GestureController;
        #endregion

        #region constructors
        private KinectSensorModul()
        {
            // Load Skeleton Drwaing Configuration
            this.ssc = new SkeletonStreamConfiguration();
            // Create the drawing group we'll use for drawing
            this._drawingGroup = new DrawingGroup();
            KinectSensor.KinectSensors.StatusChanged += new EventHandler<StatusChangedEventArgs>(KinectSensors_StatusChanged);
            // for Gesture Detection Purposes
            //this._GestureController = new GestureController();
            //this._GestureController.GestureRecognized += new EventHandler<GestureEventArgs>(_GestureController_GestureRecognized);
            DiscoverKinectSensor();
        }

   

      
        void KinectSensors_StatusChanged(object sender, StatusChangedEventArgs e)
        {
            switch (e.Status)
            {
                case KinectStatus.Initializing:
                case KinectStatus.Connected:
                    this.Kinect = e.Sensor;
                    break;
                case KinectStatus.Disconnected:
                    //TODO: Give the user feedback to plug-in a Kinect device.     
                    this.Kinect = null;
                    break;
                default:
                    //TODO: Show an error state 
                    break;
            } 
        }

        ~KinectSensorModul()
        {
            UninitializeKinectSensor(this.Kinect);
        }
        #endregion  

        #region Properties
        //Gesture Detection


        //Sensor
        private KinectSensor _Kinect;
        public KinectSensor Kinect
        {
            get { return this._Kinect; }
            set
            {
                if (this._Kinect != value)  // Is there a new source available?
                {
                    if (this._Kinect != null)
                    {
                        UninitializeKinectSensor(this._Kinect);
                        this._Kinect = null;
                    }

                    if (value != null && value.Status == KinectStatus.Connected)
                    {
                        this._Kinect = value;
                        InitializeKinectSensor(this._Kinect);
                    }
                }
            }
        }

        public WriteableBitmap ColorStreamImageProp
        {
            get { return this._ColorImageBitmap; }   
            
        }
        public ImageSource SkeletonFrameProp
        {
            get { return this._depthImageSource; }

        }

        #endregion 

        #region Events
            #region ColorStreamEvent
            public delegate void ColorStreamReadyEvent(object sender, BitmapSource colorFrame);
            public event ColorStreamReadyEvent OnColorstreamReadyEvent;
            private void ColorStreamReady(BitmapSource colorFrame)
            {

                if (OnColorstreamReadyEvent != null)
                {
                    OnColorstreamReadyEvent(this, colorFrame);
                }
            }
            #endregion

            #region DepthStreamEvent
            public delegate void DepthStreamReadyEvent(object sender, BitmapSource colorFrame);
            public event DepthStreamReadyEvent OnDepthStreamReadyEvent;
            private void DepthStreamReady(BitmapSource depthFrame)
            {

                if (OnDepthStreamReadyEvent != null)
                {
                    OnDepthStreamReadyEvent(this, depthFrame);
                }
            }
            #endregion

            #region SkeletonframeEvent
            public delegate void SkeletonFrameReadyEvent(object sender, ImageSource skeletonFrame);
            public event SkeletonFrameReadyEvent OnSkeletonFrameReadyEvent;
            private void SkeletonFrameReadyE(ImageSource skeletonFrame)
            {

                if (OnSkeletonFrameReadyEvent != null)
                {
                    OnSkeletonFrameReadyEvent(this, skeletonFrame);
                }
            }
            #endregion

            #region SkeletonTrackedEvent
            public delegate void SkeletonTrackedStateReadyEvent(object sender, bool trackedState);
            public event SkeletonTrackedStateReadyEvent OnSkeletonTrackedStateReadyEvent;
            private void SkeletonTrackedStateReady(bool trackedState)
            {

                if (OnSkeletonTrackedStateReadyEvent != null)
                {
                    OnSkeletonTrackedStateReadyEvent(this, trackedState);
                }
            }
            #endregion

            #region Gesture Detection Interface Event (provides raw Data for external Connection)
            public delegate void GestureDetectionInterfaceEvent(object sender, Skeleton[] skeletons, long frameTimestamp);
            public event GestureDetectionInterfaceEvent OnGestureDetectionInterfaceEvent;
            private void GestureDetectionInterfaceReady(Skeleton[] skeletons, long frameTimestamp)
            {

                if (OnGestureDetectionInterfaceEvent != null)
                {
                    OnGestureDetectionInterfaceEvent(this, skeletons, frameTimestamp);
                }
            }
            #endregion

            #region Gesture Detection  Event (forwarding event if a gesture is detected)
            public delegate void GestureDetectionEvent(object sender, String GestureType);
            public event GestureDetectionEvent OnGestureDetectionEvent;
            private void GestureDetectionReady(String GestureType)
            {

                if (OnGestureDetectionEvent != null)
                {
                    OnGestureDetectionEvent(this,  GestureType);
                }
            }

            //void _GestureController_GestureRecognized(object sender, GestureEventArgs e)
            //{
            //    GestureDetectionReady(e.GestureType.ToString());
            //}

            #endregion

            #region DepthFrameEvent
            public delegate void DepthFrameReadyEvent(object sender, DepthImageFrame depthFrame);
            public event DepthFrameReadyEvent OnDepthFrameReadyEvent;
            private void DepthFrameReady(DepthImageFrame depthFrame)
            {

                if (OnDepthFrameReadyEvent != null)
                {
                    OnDepthFrameReadyEvent(this, depthFrame);
                }
            }
            #endregion  

          
        #endregion

        #region Un-/Init & Discover

          private void InitializeKinectSensor(KinectSensor sensor)
        {
            if (sensor != null)
            {

                // +++++ Skeleton Stream Init ++++++++++
                var parameters = new TransformSmoothParameters
                {
                    Smoothing = 0.3f,
                    Correction = 0.0f,
                    Prediction = 0.0f,
                    JitterRadius = 1.0f,
                    MaxDeviationRadius = 0.5f
                };
                sensor.SkeletonStream.Enable(parameters);
                sensor.SkeletonStream.Enable();
                sensor.SkeletonStream.EnableTrackingInNearRange = true;
                sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
                // Turn on the skeleton stream to receive skeleton frames
                Kinect.SkeletonStream.Enable();
                // Add an event handler to be called whenever there is new color frame data
                Kinect.SkeletonFrameReady += this.SkeletonFrameReady;
                
                
                // +++++ Color Stream Init ++++++++++
                ColorImageStream colorStream = sensor.ColorStream;
                sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                this._ColorImageBitmap = new WriteableBitmap(colorStream.FrameWidth,
                                                     colorStream.FrameHeight, 96, 96,
                                                     PixelFormats.Bgr32, null);
                this._ColorImageBitmapRect = new Int32Rect(0, 0, colorStream.FrameWidth,
                                                            colorStream.FrameHeight);
                this._ColorImageStride = colorStream.FrameWidth * colorStream.FrameBytesPerPixel;
                sensor.ColorFrameReady += ColorFrameReady;



                // +++++ Depth Stream Init+++++
                sensor.DepthStream.Enable();
                this._LastDepthPixelData = new short[sensor.DepthStream.FramePixelDataLength];
                sensor.DepthFrameReady += DepthFrameReady;
                // Activate NearMode = depth range to 400mm (1’3.75”) – 3000mm (9’10.11”).
                //          Normal   = range to 800mm (2’7.5”) – 4000mm (13’1.48”)
                try
                {
                    sensor.DepthStream.Range = DepthRange.Near;
                }
                catch (InvalidOperationException e)
                {
                    MessageBox.Show(e.Message);
                }

       
                sensor.Start();
            }
        }

          private void UninitializeKinectSensor(KinectSensor sensor)
        {
            if (sensor != null)
            {
                
                //unsubscribe event handlers
                sensor.ColorFrameReady -= ColorFrameReady;
                sensor.DepthFrameReady -= DepthFrameReady;
                sensor.SkeletonFrameReady -= SkeletonFrameReady;
                //disable streams 
                sensor.ColorStream.Disable();
                sensor.DepthStream.Disable();
                sensor.SkeletonStream.Disable();
                //stop sensor
                sensor.Stop();
            }
        }

          private void DiscoverKinectSensor()
        {
            // KinectSensor.KinectSensors.StatusChanged += KinectSensors.StatusChanged;
            this.Kinect = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);
        }

        #endregion
  
        #region Kinect Stream Decoder Functions
            
          #region Skeleton Frame Ready 
           private void SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
            {
                Skeleton[] _skeletons = new Skeleton[0];

                using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
                {
                    if (skeletonFrame != null)
                    {
                        _skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                        skeletonFrame.CopySkeletonDataTo(_skeletons);

                        // Raise event for Gesture Detection Interface 
                        GestureDetectionInterfaceReady(_skeletons, skeletonFrame.Timestamp);
                        // Update Gesture Recognition
                        UpdateGestureDetection(_skeletons);

                    }
                }

                using (DrawingContext dc = this._drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, ssc.RenderWidth, ssc.RenderWidth));

                    if (_skeletons.Length != 0)
                    {
                        foreach (Skeleton skel in _skeletons)
                        {
                            RenderClippedEdges(skel, dc);

                            if (skel.TrackingState == SkeletonTrackingState.Tracked)
                            {
                                this.DrawBonesAndJoints(skel, dc);
                               
                            }
                            else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
                            {
                                dc.DrawEllipse(
                                this.ssc.centerPointBrush,
                                null,
                                this.SkeletonPointToScreen(skel.Position),
                                ssc.BodyCenterThickness,
                                ssc.BodyCenterThickness);
                            }
                        }
                    }

                    // prevent drawing outside of our render area
                    this._drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, ssc.RenderWidth, ssc.RenderWidth));
                    _depthImageSource = new DrawingImage(this._drawingGroup);
                    SkeletonFrameReadyE(_depthImageSource);
                }
          }

            /// <summary>
            /// Draws a skeleton's bones and joints
            /// </summary>
            /// <param name="skeleton">skeleton to draw</param>
            /// <param name="drawingContext">drawing context to draw to</param>
            private void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
            {
                // Render Torso
                this.DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
                this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
                this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
                this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
                this.DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
                this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
                this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);

                // Left Arm
                this.DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
                this.DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
                this.DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);

                // Right Arm
                this.DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
                this.DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
                this.DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);

                // Left Leg
                this.DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
                this.DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
                this.DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);

                // Right Leg
                this.DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
                this.DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
                this.DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);

                // Render Joints
                foreach (Joint joint in skeleton.Joints)
                {
                    Brush drawBrush = null;

                    if (joint.TrackingState == JointTrackingState.Tracked)
                    {
                        drawBrush = this.ssc.trackedJointBrush;
                    }
                    else if (joint.TrackingState == JointTrackingState.Inferred)
                    {
                        drawBrush = this.ssc.inferredJointBrush;
                    }

                    if (drawBrush != null)
                    {
                        drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), ssc.JointThickness, ssc.JointThickness);
                    }
                }
            }

            /// <summary>
            /// Maps a SkeletonPoint to lie within our render space and converts to Point
            /// </summary>
            /// <param name="skelpoint">point to map</param>
            /// <returns>mapped point</returns>
            private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
            {
                // Convert point to depth space.  
                // We are not using depth directly, but we do want the points in our 640x480 output resolution.
                DepthImagePoint depthPoint = this.Kinect.MapSkeletonPointToDepth(
                                                                                 skelpoint,
                                                                                 DepthImageFormat.Resolution640x480Fps30);
                return new Point(depthPoint.X, depthPoint.Y);
            }

            /// <summary>
            /// Draws a bone line between two joints
            /// </summary>
            /// <param name="skeleton">skeleton to draw bones from</param>
            /// <param name="drawingContext">drawing context to draw to</param>
            /// <param name="jointType0">joint to start drawing from</param>
            /// <param name="jointType1">joint to end drawing at</param>
            private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
            {
                Joint joint0 = skeleton.Joints[jointType0];
                Joint joint1 = skeleton.Joints[jointType1];

                // If we can't find either of these joints, exit
                if (joint0.TrackingState == JointTrackingState.NotTracked ||
                    joint1.TrackingState == JointTrackingState.NotTracked)
                {
                    return;
                }

                // Don't draw if both points are inferred
                if (joint0.TrackingState == JointTrackingState.Inferred &&
                    joint1.TrackingState == JointTrackingState.Inferred)
                {
                    return;
                }

                // We assume all drawn bones are inferred unless BOTH joints are tracked
                Pen drawPen = this.ssc.inferredBonePen;
                if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
                {
                    drawPen = this.ssc.trackedBonePen;
                }

                drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
            }


            /// <summary>
            /// Draws indicators to show which edges are clipping skeleton data
            /// </summary>
            /// <param name="skeleton">skeleton to draw clipping information for</param>
            /// <param name="drawingContext">drawing context to draw to</param>
            private void RenderClippedEdges(Skeleton skeleton, DrawingContext drawingContext)
            {
                if (skeleton.ClippedEdges.HasFlag(FrameEdges.Bottom))
                {
                    drawingContext.DrawRectangle(
                        Brushes.Red,
                        null,
                        new Rect(0, ssc.RenderWidth - ssc.ClipBoundsThickness, ssc.RenderWidth, ssc.ClipBoundsThickness));
                }

                if (skeleton.ClippedEdges.HasFlag(FrameEdges.Top))
                {
                    drawingContext.DrawRectangle(
                        Brushes.Red,
                        null,
                        new Rect(0, 0, ssc.RenderWidth, ssc.ClipBoundsThickness));
                }

                if (skeleton.ClippedEdges.HasFlag(FrameEdges.Left))
                {
                    drawingContext.DrawRectangle(
                        Brushes.Red,
                        null,
                        new Rect(0, 0, ssc.ClipBoundsThickness, ssc.RenderWidth));
                }

                if (skeleton.ClippedEdges.HasFlag(FrameEdges.Right))
                {
                    drawingContext.DrawRectangle(
                        Brushes.Red,
                        null,
                        new Rect(ssc.RenderWidth - ssc.ClipBoundsThickness, 0, ssc.ClipBoundsThickness, ssc.RenderWidth));
                }
            }         
          #endregion
          #region Depth Frame Ready
        private void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
            {
                using (DepthImageFrame frame = e.OpenDepthImageFrame())
                {
                    if (frame != null)
                    {
                        short[] pixelData = new short[frame.PixelDataLength];
                        frame.CopyPixelDataTo(pixelData);
                        frame.CopyPixelDataTo(this._LastDepthPixelData);
                        //Throw frame Event
                        DepthFrameReady(frame);
                        int stride = frame.Width * frame.BytesPerPixel;
                        // Throw Event
                        DepthStreamReady(BitmapSource.Create(frame.Width, frame.Height, 96, 96,
                                                                PixelFormats.Gray16, null, pixelData, stride));                 
                    }
                } 
            }
            #endregion
          #region Color Frame Ready
            private void ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
           {
            using (ColorImageFrame frame = e.OpenColorImageFrame())
            {
                if (frame != null)
                {
                    byte[] pixelData = new byte[frame.PixelDataLength];
                    frame.CopyPixelDataTo(pixelData);

                    // boost performance 
                    this._ColorImageBitmap.WritePixels(this._ColorImageBitmapRect, pixelData, this._ColorImageStride, 0);
                    // Throw Event
                    ColorStreamReady(_ColorImageBitmap);
                }
            }
        }
            #endregion
    
        #endregion

        #region Support Function

            #region Singelton Pattern Implementation
            public static KinectSensorModul Instance
            {
                get
                {
                    if (instance == null)
                    {
                        instance = new KinectSensorModul();
                    }
                    return instance;
                }
            }
            #endregion

            private void UpdateGestureDetection(Skeleton[] skeletons)
            {
                Skeleton FirstTrackedSkeleton = null;
                // prevent out of range errors
                if (skeletons != null)
                {
                    //get the first tracked skeleton
                    FirstTrackedSkeleton = (from s in skeletons
                                                     where s.TrackingState == SkeletonTrackingState.Tracked
                                                     select s).FirstOrDefault();
                }

                // Update Gesture Recognition if event is external subscribed 
                //if (this._GestureController != null && this.OnGestureDetectionEvent!=null)
                //{
                //    this._GestureController.UpdateAllGestures(FirstTrackedSkeleton);
                //}
            }

        public static UIElement DeepCopy(UIElement element)
        {
            if (element != null)
            {
                var xaml = XamlWriter.Save(element);

                var xamlString = new StringReader(xaml);

                var xmlTextReader = new XmlTextReader(xamlString);

                var deepCopyObject = (UIElement)XamlReader.Load(xmlTextReader);

                return deepCopyObject;
            }

            return null;

        }
        #endregion
    }
}
